<!-- saved from url=(0014)about:internet -->
<html>
<head>
<meta http-equiv="content-type" content="text/html; charset=utf-8" />
<title>RTW Report - ik_7dof_fourier.c</title>
<script language="JavaScript" type="text/javascript" src="rtwreport_utils.js"></script>
</head>
<body bgcolor="#eeeeee" text="#1122aa" onload="try {if (top) { if (top.rtwFileOnLoad) top.rtwFileOnLoad(window.document); else local_onload();}} catch(err) {};">
<p>
<table border="0" cellspacing="0" cellpadding="6" width="100%%" height="100%%"><tr><td width="100%%" valign="top" bgcolor="#ffffff">
<h4>File: <a href="../ik_7dof_fourier.c" target="rtwreport_document_frame" id="linkToText_plain">ik_7dof_fourier.c</a></h4>
<pre id="RTWcode">
<a name="1"><font color="#888888 "><i>    1   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="2"><font color="#888888 "><i>    2   </i></font></a><font color="#117755"><I> * File: ik_7dof_fourier.c</i></font>
<a name="3"><font color="#888888 "><i>    3   </i></font></a><font color="#117755"><I> *</i></font>
<a name="4"><font color="#888888 "><i>    4   </i></font></a><font color="#117755"><I> * MATLAB Coder version            : 3.1</i></font>
<a name="5"><font color="#888888 "><i>    5   </i></font></a><font color="#117755"><I> * C/C++ source code generated on  : 07-Oct-2023 11:12:14</i></font>
<a name="6"><font color="#888888 "><i>    6   </i></font></a><font color="#117755"><I> */</i></font>
<a name="7"><font color="#888888 "><i>    7   </i></font></a>
<a name="8"><font color="#888888 "><i>    8   </i></font></a><font color="#117755"><i>/* Include Files */</i></font>
<a name="9"><font color="#888888 "><i>    9   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;rt_nonfinite.h&quot;</font>
<a name="10"><font color="#888888 "><i>   10   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;ik_7dof_fourier.h&quot;</font>
<a name="11"><font color="#888888 "><i>   11   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;norm.h&quot;</font>
<a name="12"><font color="#888888 "><i>   12   </i></font></a>
<a name="13"><font color="#888888 "><i>   13   </i></font></a><font color="#117755"><i>/* Function Declarations */</i></font>
<a name="14"><font color="#888888 "><i>   14   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>double</b></font> <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(<font color="#112266"><b>double</b></font> u0, <font color="#112266"><b>double</b></font> u1);
<a name="15"><font color="#888888 "><i>   15   </i></font></a>
<a name="16"><font color="#888888 "><i>   16   </i></font></a><font color="#117755"><i>/* Function Definitions */</i></font>
<a name="17"><font color="#888888 "><i>   17   </i></font></a>
<a name="18"><font color="#888888 "><i>   18   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="19"><font color="#888888 "><i>   19   </i></font></a><font color="#117755"><I> * Arguments    : double u0</i></font>
<a name="20"><font color="#888888 "><i>   20   </i></font></a><font color="#117755"><I> *                double u1</i></font>
<a name="21"><font color="#888888 "><i>   21   </i></font></a><font color="#117755"><I> * Return Type  : double</i></font>
<a name="22"><font color="#888888 "><i>   22   </i></font></a><font color="#117755"><I> */</i></font>
<a name="23"><font color="#888888 "><i>   23   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>double</b></font> <a name="fcn_rt_atan2d_snf">rt_atan2d_snf</a>(<font color="#112266"><b>double</b></font> u0, <font color="#112266"><b>double</b></font> u1)
<a name="24"><font color="#888888 "><i>   24   </i></font></a><b>{</b>
<a name="25"><font color="#888888 "><i>   25   </i></font></a>  <font color="#112266"><b>double</b></font> y;
<a name="26"><font color="#888888 "><i>   26   </i></font></a>  <font color="#112266"><b>int</b></font> b_u0;
<a name="27"><font color="#888888 "><i>   27   </i></font></a>  <font color="#112266"><b>int</b></font> b_u1;
<a name="28"><font color="#888888 "><i>   28   </i></font></a>  <font color="#112266"><b>if</b></font> (<a href="rt_nonfinite_c.html#fcn_rtIsNaN" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaN');" target="_self"><font color="#1122aa">rtIsNaN</font></a>(u0) || <a href="rt_nonfinite_c.html#fcn_rtIsNaN" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsNaN');" target="_self"><font color="#1122aa">rtIsNaN</font></a>(u1)) <b>{</b>
<a name="29"><font color="#888888 "><i>   29   </i></font></a>    y = <a href="rt_nonfinite_c.html#var_rtNaN" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_rtNaN');" target="_self"><font color="#1122aa">rtNaN</font></a>;
<a name="30"><font color="#888888 "><i>   30   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> (<a href="rt_nonfinite_c.html#fcn_rtIsInf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsInf');" target="_self"><font color="#1122aa">rtIsInf</font></a>(u0) &amp;&amp; <a href="rt_nonfinite_c.html#fcn_rtIsInf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rtIsInf');" target="_self"><font color="#1122aa">rtIsInf</font></a>(u1)) <b>{</b>
<a name="31"><font color="#888888 "><i>   31   </i></font></a>    <font color="#112266"><b>if</b></font> (u0 &gt; 0.0) <b>{</b>
<a name="32"><font color="#888888 "><i>   32   </i></font></a>      b_u0 = 1;
<a name="33"><font color="#888888 "><i>   33   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="34"><font color="#888888 "><i>   34   </i></font></a>      b_u0 = -1;
<a name="35"><font color="#888888 "><i>   35   </i></font></a>    <b>}</b>
<a name="36"><font color="#888888 "><i>   36   </i></font></a>
<a name="37"><font color="#888888 "><i>   37   </i></font></a>    <font color="#112266"><b>if</b></font> (u1 &gt; 0.0) <b>{</b>
<a name="38"><font color="#888888 "><i>   38   </i></font></a>      b_u1 = 1;
<a name="39"><font color="#888888 "><i>   39   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="40"><font color="#888888 "><i>   40   </i></font></a>      b_u1 = -1;
<a name="41"><font color="#888888 "><i>   41   </i></font></a>    <b>}</b>
<a name="42"><font color="#888888 "><i>   42   </i></font></a>
<a name="43"><font color="#888888 "><i>   43   </i></font></a>    y = atan2(b_u0, b_u1);
<a name="44"><font color="#888888 "><i>   44   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> (u1 == 0.0) <b>{</b>
<a name="45"><font color="#888888 "><i>   45   </i></font></a>    <font color="#112266"><b>if</b></font> (u0 &gt; 0.0) <b>{</b>
<a name="46"><font color="#888888 "><i>   46   </i></font></a>      y = RT_PI / 2.0;
<a name="47"><font color="#888888 "><i>   47   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> (u0 &lt; 0.0) <b>{</b>
<a name="48"><font color="#888888 "><i>   48   </i></font></a>      y = -(RT_PI / 2.0);
<a name="49"><font color="#888888 "><i>   49   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="50"><font color="#888888 "><i>   50   </i></font></a>      y = 0.0;
<a name="51"><font color="#888888 "><i>   51   </i></font></a>    <b>}</b>
<a name="52"><font color="#888888 "><i>   52   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="53"><font color="#888888 "><i>   53   </i></font></a>    y = atan2(u0, u1);
<a name="54"><font color="#888888 "><i>   54   </i></font></a>  <b>}</b>
<a name="55"><font color="#888888 "><i>   55   </i></font></a>
<a name="56"><font color="#888888 "><i>   56   </i></font></a>  <font color="#112266"><b>return</b></font> y;
<a name="57"><font color="#888888 "><i>   57   </i></font></a><b>}</b>
<a name="58"><font color="#888888 "><i>   58   </i></font></a>
<a name="59"><font color="#888888 "><i>   59   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="60"><font color="#888888 "><i>   60   </i></font></a><font color="#117755"><I> * theta = ik_7dof_fourier(R_x,R_y,R_z,p_x,p_y,p_z,bet)</i></font>
<a name="61"><font color="#888888 "><i>   61   </i></font></a><font color="#117755"><I> *  [q1,q2,q3,q4,q5,q6,q7] = ik_7dof_fourier(R_x,R_y,R_z,p_x,p_y,p_z,bet)</i></font>
<a name="62"><font color="#888888 "><i>   62   </i></font></a><font color="#117755"><I> * Arguments    : double R_x</i></font>
<a name="63"><font color="#888888 "><i>   63   </i></font></a><font color="#117755"><I> *                double R_y</i></font>
<a name="64"><font color="#888888 "><i>   64   </i></font></a><font color="#117755"><I> *                double R_z</i></font>
<a name="65"><font color="#888888 "><i>   65   </i></font></a><font color="#117755"><I> *                double p_x</i></font>
<a name="66"><font color="#888888 "><i>   66   </i></font></a><font color="#117755"><I> *                double p_y</i></font>
<a name="67"><font color="#888888 "><i>   67   </i></font></a><font color="#117755"><I> *                double p_z</i></font>
<a name="68"><font color="#888888 "><i>   68   </i></font></a><font color="#117755"><I> *                double bet</i></font>
<a name="69"><font color="#888888 "><i>   69   </i></font></a><font color="#117755"><I> *                double theta[7]</i></font>
<a name="70"><font color="#888888 "><i>   70   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="71"><font color="#888888 "><i>   71   </i></font></a><font color="#117755"><I> */</i></font>
<a name="72"><font color="#888888 "><i>   72   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_ik_7dof_fourier">ik_7dof_fourier</a>(<font color="#112266"><b>double</b></font> R_x, <font color="#112266"><b>double</b></font> R_y, <font color="#112266"><b>double</b></font> R_z, <font color="#112266"><b>double</b></font> p_x, <font color="#112266"><b>double</b></font> p_y,
<a name="73"><font color="#888888 "><i>   73   </i></font></a>                     <font color="#112266"><b>double</b></font> p_z, <font color="#112266"><b>double</b></font> bet, <font color="#112266"><b>double</b></font> theta[7])
<a name="74"><font color="#888888 "><i>   74   </i></font></a><b>{</b>
<a name="75"><font color="#888888 "><i>   75   </i></font></a>  <font color="#112266"><b>double</b></font> p[3];
<a name="76"><font color="#888888 "><i>   76   </i></font></a>  <font color="#112266"><b>double</b></font> n[3];
<a name="77"><font color="#888888 "><i>   77   </i></font></a>  <font color="#112266"><b>double</b></font> y;
<a name="78"><font color="#888888 "><i>   78   </i></font></a>  <font color="#112266"><b>double</b></font> b_y;
<a name="79"><font color="#888888 "><i>   79   </i></font></a>  <font color="#112266"><b>double</b></font> p_e[3];
<a name="80"><font color="#888888 "><i>   80   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="81"><font color="#888888 "><i>   81   </i></font></a>  <font color="#112266"><b>double</b></font> b_p_e[9];
<a name="82"><font color="#888888 "><i>   82   </i></font></a>  <font color="#112266"><b>double</b></font> Rk0x[3];
<a name="83"><font color="#888888 "><i>   83   </i></font></a>  <font color="#112266"><b>double</b></font> n4[3];
<a name="84"><font color="#888888 "><i>   84   </i></font></a>  <font color="#112266"><b>int</b></font> i0;
<a name="85"><font color="#888888 "><i>   85   </i></font></a>  <font color="#112266"><b>double</b></font> cB;
<a name="86"><font color="#888888 "><i>   86   </i></font></a>  <font color="#112266"><b>double</b></font> sB;
<a name="87"><font color="#888888 "><i>   87   </i></font></a>  <font color="#112266"><b>double</b></font> b_Rk0x[9];
<a name="88"><font color="#888888 "><i>   88   </i></font></a>  <font color="#112266"><b>double</b></font> c_Rk0x[9];
<a name="89"><font color="#888888 "><i>   89   </i></font></a>  <font color="#112266"><b>double</b></font> b_n;
<a name="90"><font color="#888888 "><i>   90   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv0[3] = <b>{</b> 0, 0, 1 <b>}</b>;
<a name="91"><font color="#888888 "><i>   91   </i></font></a>
<a name="92"><font color="#888888 "><i>   92   </i></font></a>  <font color="#112266"><b>double</b></font> x;
<a name="93"><font color="#888888 "><i>   93   </i></font></a>  <font color="#112266"><b>double</b></font> theta2;
<a name="94"><font color="#888888 "><i>   94   </i></font></a>  <font color="#112266"><b>int</b></font> i1;
<a name="95"><font color="#888888 "><i>   95   </i></font></a>  <font color="#112266"><b>double</b></font> cond_BE;
<a name="96"><font color="#888888 "><i>   96   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>double</b></font> b[3] = <b>{</b> 249.72434803198504, 0.0, 0.0 <b>}</b>;
<a name="97"><font color="#888888 "><i>   97   </i></font></a>
<a name="98"><font color="#888888 "><i>   98   </i></font></a>  <font color="#112266"><b>double</b></font> theta1;
<a name="99"><font color="#888888 "><i>   99   </i></font></a>  <font color="#112266"><b>double</b></font> dv0[16];
<a name="100"><font color="#888888 "><i>  100   </i></font></a>  <font color="#112266"><b>double</b></font> dv1[16];
<a name="101"><font color="#888888 "><i>  101   </i></font></a>  <font color="#112266"><b>double</b></font> T02[4];
<a name="102"><font color="#888888 "><i>  102   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv1[4] = <b>{</b> 0, 0, 1, 0 <b>}</b>;
<a name="103"><font color="#888888 "><i>  103   </i></font></a>
<a name="104"><font color="#888888 "><i>  104   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv2[4] = <b>{</b> 0, 0, 0, 1 <b>}</b>;
<a name="105"><font color="#888888 "><i>  105   </i></font></a>
<a name="106"><font color="#888888 "><i>  106   </i></font></a>  <font color="#112266"><b>double</b></font> b_T02[16];
<a name="107"><font color="#888888 "><i>  107   </i></font></a>  <font color="#112266"><b>double</b></font> theta4;
<a name="108"><font color="#888888 "><i>  108   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> b_b[4] = <b>{</b> 40, 0, 0, 1 <b>}</b>;
<a name="109"><font color="#888888 "><i>  109   </i></font></a>
<a name="110"><font color="#888888 "><i>  110   </i></font></a>  <font color="#112266"><b>double</b></font> theta3;
<a name="111"><font color="#888888 "><i>  111   </i></font></a>  <font color="#112266"><b>double</b></font> c_T02[16];
<a name="112"><font color="#888888 "><i>  112   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>double</b></font> dv2[4] = <b>{</b> 0.0, 0.0, -1.0, -246.5 <b>}</b>;
<a name="113"><font color="#888888 "><i>  113   </i></font></a>
<a name="114"><font color="#888888 "><i>  114   </i></font></a>  <font color="#112266"><b>double</b></font> dv3[9];
<a name="115"><font color="#888888 "><i>  115   </i></font></a>  <font color="#112266"><b>double</b></font> theta6;
<a name="116"><font color="#888888 "><i>  116   </i></font></a>  <font color="#112266"><b>double</b></font> R47[9];
<a name="117"><font color="#888888 "><i>  117   </i></font></a>  <font color="#112266"><b>double</b></font> theta5;
<a name="118"><font color="#888888 "><i>  118   </i></font></a>  <font color="#112266"><b>double</b></font> theta7;
<a name="119"><font color="#888888 "><i>  119   </i></font></a>
<a name="120"><font color="#888888 "><i>  120   </i></font></a>  <font color="#117755"><i>/*  机械臂尺寸参数 */</i></font>
<a name="121"><font color="#888888 "><i>  121   </i></font></a>  <font color="#117755"><i>/*  R07 = [1,0,0; */</i></font>
<a name="122"><font color="#888888 "><i>  122   </i></font></a>  <font color="#117755"><i>/*         0,1,0; */</i></font>
<a name="123"><font color="#888888 "><i>  123   </i></font></a>  <font color="#117755"><i>/*         0,0,1]; */</i></font>
<a name="124"><font color="#888888 "><i>  124   </i></font></a>  p[0] = p_x;
<a name="125"><font color="#888888 "><i>  125   </i></font></a>  p[1] = p_y;
<a name="126"><font color="#888888 "><i>  126   </i></font></a>  p[2] = p_z;
<a name="127"><font color="#888888 "><i>  127   </i></font></a>  n[0] = p_y * -0.0 - p_z * -0.0;
<a name="128"><font color="#888888 "><i>  128   </i></font></a>  n[1] = -p_z - p_x * -0.0;
<a name="129"><font color="#888888 "><i>  129   </i></font></a>  n[2] = p_x * -0.0 - (-p_y);
<a name="130"><font color="#888888 "><i>  130   </i></font></a>
<a name="131"><font color="#888888 "><i>  131   </i></font></a>  <font color="#117755"><i>/*  零位面法向量为n */</i></font>
<a name="132"><font color="#888888 "><i>  132   </i></font></a>  y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(n);
<a name="133"><font color="#888888 "><i>  133   </i></font></a>  b_y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(p);
<a name="134"><font color="#888888 "><i>  134   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="135"><font color="#888888 "><i>  135   </i></font></a>    p_e[i] = p[i] / b_y;
<a name="136"><font color="#888888 "><i>  136   </i></font></a>    n[i] /= y;
<a name="137"><font color="#888888 "><i>  137   </i></font></a>  <b>}</b>
<a name="138"><font color="#888888 "><i>  138   </i></font></a>
<a name="139"><font color="#888888 "><i>  139   </i></font></a>  <font color="#117755"><i>/*  归一化的p */</i></font>
<a name="140"><font color="#888888 "><i>  140   </i></font></a>  b_p_e[0] = p_e[0] * p_e[0] * (1.0 - cos(bet)) + cos(bet);
<a name="141"><font color="#888888 "><i>  141   </i></font></a>  b_p_e[3] = p_e[0] * p_e[1] * (1.0 - cos(bet)) - p_e[2] * sin(bet);
<a name="142"><font color="#888888 "><i>  142   </i></font></a>  b_p_e[6] = p_e[0] * p_e[2] * (1.0 - cos(bet)) + p_e[1] * sin(bet);
<a name="143"><font color="#888888 "><i>  143   </i></font></a>  b_p_e[1] = p_e[0] * p_e[1] * (1.0 - cos(bet)) + p_e[2] * sin(bet);
<a name="144"><font color="#888888 "><i>  144   </i></font></a>  b_p_e[4] = p_e[1] * p_e[1] * (1.0 - cos(bet)) + cos(bet);
<a name="145"><font color="#888888 "><i>  145   </i></font></a>  b_p_e[7] = p_e[1] * p_e[2] * (1.0 - cos(bet)) - p_e[0] * sin(bet);
<a name="146"><font color="#888888 "><i>  146   </i></font></a>  b_p_e[2] = p_e[0] * p_e[2] * (1.0 - cos(bet)) - p_e[1] * sin(bet);
<a name="147"><font color="#888888 "><i>  147   </i></font></a>  b_p_e[5] = p_e[1] * p_e[2] * (1.0 - cos(bet)) + p_e[0] * sin(bet);
<a name="148"><font color="#888888 "><i>  148   </i></font></a>  b_p_e[8] = p_e[2] * p_e[2] * (1.0 - cos(bet)) + cos(bet);
<a name="149"><font color="#888888 "><i>  149   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="150"><font color="#888888 "><i>  150   </i></font></a>    n4[i] = 0.0;
<a name="151"><font color="#888888 "><i>  151   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="152"><font color="#888888 "><i>  152   </i></font></a>      n4[i] += b_p_e[i + 3 * i0] * n[i0];
<a name="153"><font color="#888888 "><i>  153   </i></font></a>    <b>}</b>
<a name="154"><font color="#888888 "><i>  154   </i></font></a>  <b>}</b>
<a name="155"><font color="#888888 "><i>  155   </i></font></a>
<a name="156"><font color="#888888 "><i>  156   </i></font></a>  Rk0x[0] = p_e[0];
<a name="157"><font color="#888888 "><i>  157   </i></font></a>  Rk0x[1] = p_e[1];
<a name="158"><font color="#888888 "><i>  158   </i></font></a>  Rk0x[2] = p_e[2];
<a name="159"><font color="#888888 "><i>  159   </i></font></a>  p_e[0] = n4[0];
<a name="160"><font color="#888888 "><i>  160   </i></font></a>  p_e[1] = n4[1];
<a name="161"><font color="#888888 "><i>  161   </i></font></a>  p_e[2] = n4[2];
<a name="162"><font color="#888888 "><i>  162   </i></font></a>  n[0] = n4[1] * Rk0x[2] - n4[2] * Rk0x[1];
<a name="163"><font color="#888888 "><i>  163   </i></font></a>  n[1] = n4[2] * Rk0x[0] - n4[0] * Rk0x[2];
<a name="164"><font color="#888888 "><i>  164   </i></font></a>  n[2] = n4[0] * Rk0x[1] - n4[1] * Rk0x[0];
<a name="165"><font color="#888888 "><i>  165   </i></font></a>
<a name="166"><font color="#888888 "><i>  166   </i></font></a>  <font color="#117755"><i>/* 得到k轴方向的y在0中的描述 */</i></font>
<a name="167"><font color="#888888 "><i>  167   </i></font></a>  y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(n);
<a name="168"><font color="#888888 "><i>  168   </i></font></a>
<a name="169"><font color="#888888 "><i>  169   </i></font></a>  <font color="#117755"><i>/* 归一化，真正得到k轴的y在0中的描述 */</i></font>
<a name="170"><font color="#888888 "><i>  170   </i></font></a>  b_y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(p);
<a name="171"><font color="#888888 "><i>  171   </i></font></a>  cB = ((62362.249999999993 + b_y * b_y) - 58322.25) / (499.44869606397009 *
<a name="172"><font color="#888888 "><i>  172   </i></font></a>    <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(p));
<a name="173"><font color="#888888 "><i>  173   </i></font></a>  sB = sqrt(1.0 - cB * cB);
<a name="174"><font color="#888888 "><i>  174   </i></font></a>  b_p_e[0] = cB;
<a name="175"><font color="#888888 "><i>  175   </i></font></a>  b_p_e[3] = -sB;
<a name="176"><font color="#888888 "><i>  176   </i></font></a>  b_p_e[6] = 0.0;
<a name="177"><font color="#888888 "><i>  177   </i></font></a>  b_p_e[1] = sB;
<a name="178"><font color="#888888 "><i>  178   </i></font></a>  b_p_e[4] = cB;
<a name="179"><font color="#888888 "><i>  179   </i></font></a>  b_p_e[7] = 0.0;
<a name="180"><font color="#888888 "><i>  180   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="181"><font color="#888888 "><i>  181   </i></font></a>    b_n = n[i] / y;
<a name="182"><font color="#888888 "><i>  182   </i></font></a>    b_Rk0x[i] = Rk0x[i];
<a name="183"><font color="#888888 "><i>  183   </i></font></a>    b_Rk0x[3 + i] = b_n;
<a name="184"><font color="#888888 "><i>  184   </i></font></a>    b_Rk0x[6 + i] = p_e[i];
<a name="185"><font color="#888888 "><i>  185   </i></font></a>    b_p_e[2 + 3 * i] = iv0[i];
<a name="186"><font color="#888888 "><i>  186   </i></font></a>    n[i] = b_n;
<a name="187"><font color="#888888 "><i>  187   </i></font></a>  <b>}</b>
<a name="188"><font color="#888888 "><i>  188   </i></font></a>
<a name="189"><font color="#888888 "><i>  189   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="190"><font color="#888888 "><i>  190   </i></font></a>    n4[i] = 0.0;
<a name="191"><font color="#888888 "><i>  191   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="192"><font color="#888888 "><i>  192   </i></font></a>      c_Rk0x[i + 3 * i0] = 0.0;
<a name="193"><font color="#888888 "><i>  193   </i></font></a>      <font color="#112266"><b>for</b></font> (i1 = 0; i1 &lt; 3; i1++) <b>{</b>
<a name="194"><font color="#888888 "><i>  194   </i></font></a>        c_Rk0x[i + 3 * i0] += b_Rk0x[i + 3 * i1] * b_p_e[i1 + 3 * i0];
<a name="195"><font color="#888888 "><i>  195   </i></font></a>      <b>}</b>
<a name="196"><font color="#888888 "><i>  196   </i></font></a>
<a name="197"><font color="#888888 "><i>  197   </i></font></a>      n4[i] += c_Rk0x[i + 3 * i0] * b[i0];
<a name="198"><font color="#888888 "><i>  198   </i></font></a>    <b>}</b>
<a name="199"><font color="#888888 "><i>  199   </i></font></a>  <b>}</b>
<a name="200"><font color="#888888 "><i>  200   </i></font></a>
<a name="201"><font color="#888888 "><i>  201   </i></font></a>  <font color="#117755"><i>/*  E点坐标在0系下的表示, xE=pE0(1), yE=pE0(2), zE=pE0(3) */</i></font>
<a name="202"><font color="#888888 "><i>  202   </i></font></a>  <font color="#117755"><i>/*  角fai */</i></font>
<a name="203"><font color="#888888 "><i>  203   </i></font></a>  y = n4[2] / 249.72434803198504;
<a name="204"><font color="#888888 "><i>  204   </i></font></a>  x = acos(y);
<a name="205"><font color="#888888 "><i>  205   </i></font></a>  theta2 = acos(y) - 0.16086957252543341;
<a name="206"><font color="#888888 "><i>  206   </i></font></a>
<a name="207"><font color="#888888 "><i>  207   </i></font></a>  <font color="#117755"><i>/*  以上求得theta2  */</i></font>
<a name="208"><font color="#888888 "><i>  208   </i></font></a>  cond_BE = 40.0 * cos(theta2) + 246.5 * sin(theta2);
<a name="209"><font color="#888888 "><i>  209   </i></font></a>  <font color="#112266"><b>if</b></font> (fabs(cond_BE) &gt; 1.0E-6) <b>{</b>
<a name="210"><font color="#888888 "><i>  210   </i></font></a>    theta1 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(n4[1] / cond_BE, n4[0] / cond_BE);
<a name="211"><font color="#888888 "><i>  211   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="212"><font color="#888888 "><i>  212   </i></font></a>    theta1 = -0.16086957252543341;
<a name="213"><font color="#888888 "><i>  213   </i></font></a>  <b>}</b>
<a name="214"><font color="#888888 "><i>  214   </i></font></a>
<a name="215"><font color="#888888 "><i>  215   </i></font></a>  <font color="#117755"><i>/*  theta1 = pi - theta1; %不知道发生了什么，这里反一下就好了 */</i></font>
<a name="216"><font color="#888888 "><i>  216   </i></font></a>  <font color="#117755"><i>/*  以上求得theta1 */</i></font>
<a name="217"><font color="#888888 "><i>  217   </i></font></a>  dv0[0] = cos(theta1);
<a name="218"><font color="#888888 "><i>  218   </i></font></a>  dv0[4] = -sin(theta1);
<a name="219"><font color="#888888 "><i>  219   </i></font></a>  dv0[8] = 0.0;
<a name="220"><font color="#888888 "><i>  220   </i></font></a>  dv0[12] = 0.0;
<a name="221"><font color="#888888 "><i>  221   </i></font></a>  dv0[1] = sin(theta1);
<a name="222"><font color="#888888 "><i>  222   </i></font></a>  dv0[5] = cos(theta1);
<a name="223"><font color="#888888 "><i>  223   </i></font></a>  dv0[9] = 0.0;
<a name="224"><font color="#888888 "><i>  224   </i></font></a>  dv0[13] = 0.0;
<a name="225"><font color="#888888 "><i>  225   </i></font></a>  dv1[0] = cos(theta2);
<a name="226"><font color="#888888 "><i>  226   </i></font></a>  dv1[4] = -sin(theta2);
<a name="227"><font color="#888888 "><i>  227   </i></font></a>  dv1[8] = 0.0;
<a name="228"><font color="#888888 "><i>  228   </i></font></a>  dv1[12] = 0.0;
<a name="229"><font color="#888888 "><i>  229   </i></font></a>  dv1[2] = -sin(theta2);
<a name="230"><font color="#888888 "><i>  230   </i></font></a>  dv1[6] = -cos(theta2);
<a name="231"><font color="#888888 "><i>  231   </i></font></a>  dv1[10] = 0.0;
<a name="232"><font color="#888888 "><i>  232   </i></font></a>  dv1[14] = 0.0;
<a name="233"><font color="#888888 "><i>  233   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="234"><font color="#888888 "><i>  234   </i></font></a>    dv0[2 + (i &lt;&lt; 2)] = iv1[i];
<a name="235"><font color="#888888 "><i>  235   </i></font></a>    dv0[3 + (i &lt;&lt; 2)] = iv2[i];
<a name="236"><font color="#888888 "><i>  236   </i></font></a>    dv1[1 + (i &lt;&lt; 2)] = iv1[i];
<a name="237"><font color="#888888 "><i>  237   </i></font></a>    dv1[3 + (i &lt;&lt; 2)] = iv2[i];
<a name="238"><font color="#888888 "><i>  238   </i></font></a>  <b>}</b>
<a name="239"><font color="#888888 "><i>  239   </i></font></a>
<a name="240"><font color="#888888 "><i>  240   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="241"><font color="#888888 "><i>  241   </i></font></a>    T02[i] = 0.0;
<a name="242"><font color="#888888 "><i>  242   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 4; i0++) <b>{</b>
<a name="243"><font color="#888888 "><i>  243   </i></font></a>      b_T02[i + (i0 &lt;&lt; 2)] = 0.0;
<a name="244"><font color="#888888 "><i>  244   </i></font></a>      <font color="#112266"><b>for</b></font> (i1 = 0; i1 &lt; 4; i1++) <b>{</b>
<a name="245"><font color="#888888 "><i>  245   </i></font></a>        b_T02[i + (i0 &lt;&lt; 2)] += dv0[i + (i1 &lt;&lt; 2)] * dv1[i1 + (i0 &lt;&lt; 2)];
<a name="246"><font color="#888888 "><i>  246   </i></font></a>      <b>}</b>
<a name="247"><font color="#888888 "><i>  247   </i></font></a>
<a name="248"><font color="#888888 "><i>  248   </i></font></a>      T02[i] += b_T02[i + (i0 &lt;&lt; 2)] * (<font color="#112266"><b>double</b></font>)b_b[i0];
<a name="249"><font color="#888888 "><i>  249   </i></font></a>    <b>}</b>
<a name="250"><font color="#888888 "><i>  250   </i></font></a>  <b>}</b>
<a name="251"><font color="#888888 "><i>  251   </i></font></a>
<a name="252"><font color="#888888 "><i>  252   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="253"><font color="#888888 "><i>  253   </i></font></a>    p_e[i] = p[i] - T02[i];
<a name="254"><font color="#888888 "><i>  254   </i></font></a>    p[i] -= n4[i];
<a name="255"><font color="#888888 "><i>  255   </i></font></a>  <b>}</b>
<a name="256"><font color="#888888 "><i>  256   </i></font></a>
<a name="257"><font color="#888888 "><i>  257   </i></font></a>  b_y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(p_e);
<a name="258"><font color="#888888 "><i>  258   </i></font></a>  b_y *= b_y;
<a name="259"><font color="#888888 "><i>  259   </i></font></a>  <font color="#112266"><b>if</b></font> (119084.5 - b_y &gt; 1.0E-6) <b>{</b>
<a name="260"><font color="#888888 "><i>  260   </i></font></a>    theta4 = 3.1415926535897931 - acos((119084.5 - b_y) / 119059.5);
<a name="261"><font color="#888888 "><i>  261   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="262"><font color="#888888 "><i>  262   </i></font></a>    theta4 = 0.0;
<a name="263"><font color="#888888 "><i>  263   </i></font></a>  <b>}</b>
<a name="264"><font color="#888888 "><i>  264   </i></font></a>
<a name="265"><font color="#888888 "><i>  265   </i></font></a>  <font color="#117755"><i>/*  以上求得theta4 */</i></font>
<a name="266"><font color="#888888 "><i>  266   </i></font></a>  n[0] = p_e[1] * p[2] - p_e[2] * p[1];
<a name="267"><font color="#888888 "><i>  267   </i></font></a>  n[1] = p_e[2] * p[0] - p_e[0] * p[2];
<a name="268"><font color="#888888 "><i>  268   </i></font></a>  n[2] = p_e[0] * p[1] - p_e[1] * p[0];
<a name="269"><font color="#888888 "><i>  269   </i></font></a>  y = <a href="norm_c.html#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(n);
<a name="270"><font color="#888888 "><i>  270   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="271"><font color="#888888 "><i>  271   </i></font></a>    n[i] /= y;
<a name="272"><font color="#888888 "><i>  272   </i></font></a>  <b>}</b>
<a name="273"><font color="#888888 "><i>  273   </i></font></a>
<a name="274"><font color="#888888 "><i>  274   </i></font></a>  <font color="#112266"><b>if</b></font> (sin(theta2) == 0.0) <b>{</b>
<a name="275"><font color="#888888 "><i>  275   </i></font></a>    theta3 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(-n[0], n[1]) - theta1;
<a name="276"><font color="#888888 "><i>  276   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <font color="#112266"><b>if</b></font> ((fabs(sin(x - 0.16086957252543341)) &gt; 1.0E-6) &amp;&amp; (fabs(cos(theta1))
<a name="277"><font color="#888888 "><i>  277   </i></font></a>              &lt; 1.0E-6)) <b>{</b>
<a name="278"><font color="#888888 "><i>  278   </i></font></a>    theta3 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(n[2] / sin(x - 0.16086957252543341), -(n[0] + cos
<a name="279"><font color="#888888 "><i>  279   </i></font></a>      (theta1) * cos(theta2) * n[2] / sin(x - 0.16086957252543341)) / sin(theta1));
<a name="280"><font color="#888888 "><i>  280   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="281"><font color="#888888 "><i>  281   </i></font></a>    theta3 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(n[2] / sin(x - 0.16086957252543341), (n[1] + sin
<a name="282"><font color="#888888 "><i>  282   </i></font></a>      (theta1) * cos(theta2) * n[2] / sin(x - 0.16086957252543341)) / cos(theta1));
<a name="283"><font color="#888888 "><i>  283   </i></font></a>  <b>}</b>
<a name="284"><font color="#888888 "><i>  284   </i></font></a>
<a name="285"><font color="#888888 "><i>  285   </i></font></a>  <font color="#117755"><i>/*  以上求得theta3 */</i></font>
<a name="286"><font color="#888888 "><i>  286   </i></font></a>  dv0[0] = cos(theta3);
<a name="287"><font color="#888888 "><i>  287   </i></font></a>  dv0[4] = -sin(theta3);
<a name="288"><font color="#888888 "><i>  288   </i></font></a>  dv0[8] = 0.0;
<a name="289"><font color="#888888 "><i>  289   </i></font></a>  dv0[12] = 40.0;
<a name="290"><font color="#888888 "><i>  290   </i></font></a>  dv0[2] = sin(theta3);
<a name="291"><font color="#888888 "><i>  291   </i></font></a>  dv0[6] = cos(theta3);
<a name="292"><font color="#888888 "><i>  292   </i></font></a>  dv0[10] = 0.0;
<a name="293"><font color="#888888 "><i>  293   </i></font></a>  dv0[14] = 0.0;
<a name="294"><font color="#888888 "><i>  294   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="295"><font color="#888888 "><i>  295   </i></font></a>    dv0[1 + (i &lt;&lt; 2)] = dv2[i];
<a name="296"><font color="#888888 "><i>  296   </i></font></a>    dv0[3 + (i &lt;&lt; 2)] = iv2[i];
<a name="297"><font color="#888888 "><i>  297   </i></font></a>  <b>}</b>
<a name="298"><font color="#888888 "><i>  298   </i></font></a>
<a name="299"><font color="#888888 "><i>  299   </i></font></a>  dv1[0] = cos(theta4);
<a name="300"><font color="#888888 "><i>  300   </i></font></a>  dv1[4] = -sin(theta4);
<a name="301"><font color="#888888 "><i>  301   </i></font></a>  dv1[8] = 0.0;
<a name="302"><font color="#888888 "><i>  302   </i></font></a>  dv1[12] = 0.0;
<a name="303"><font color="#888888 "><i>  303   </i></font></a>  dv1[2] = -sin(theta4);
<a name="304"><font color="#888888 "><i>  304   </i></font></a>  dv1[6] = -cos(theta4);
<a name="305"><font color="#888888 "><i>  305   </i></font></a>  dv1[10] = 0.0;
<a name="306"><font color="#888888 "><i>  306   </i></font></a>  dv1[14] = 0.0;
<a name="307"><font color="#888888 "><i>  307   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="308"><font color="#888888 "><i>  308   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 4; i0++) <b>{</b>
<a name="309"><font color="#888888 "><i>  309   </i></font></a>      c_T02[i + (i0 &lt;&lt; 2)] = 0.0;
<a name="310"><font color="#888888 "><i>  310   </i></font></a>      <font color="#112266"><b>for</b></font> (i1 = 0; i1 &lt; 4; i1++) <b>{</b>
<a name="311"><font color="#888888 "><i>  311   </i></font></a>        c_T02[i + (i0 &lt;&lt; 2)] += b_T02[i + (i1 &lt;&lt; 2)] * dv0[i1 + (i0 &lt;&lt; 2)];
<a name="312"><font color="#888888 "><i>  312   </i></font></a>      <b>}</b>
<a name="313"><font color="#888888 "><i>  313   </i></font></a>    <b>}</b>
<a name="314"><font color="#888888 "><i>  314   </i></font></a>
<a name="315"><font color="#888888 "><i>  315   </i></font></a>    dv1[1 + (i &lt;&lt; 2)] = iv1[i];
<a name="316"><font color="#888888 "><i>  316   </i></font></a>    dv1[3 + (i &lt;&lt; 2)] = iv2[i];
<a name="317"><font color="#888888 "><i>  317   </i></font></a>  <b>}</b>
<a name="318"><font color="#888888 "><i>  318   </i></font></a>
<a name="319"><font color="#888888 "><i>  319   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="320"><font color="#888888 "><i>  320   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 4; i0++) <b>{</b>
<a name="321"><font color="#888888 "><i>  321   </i></font></a>      b_T02[i + (i0 &lt;&lt; 2)] = 0.0;
<a name="322"><font color="#888888 "><i>  322   </i></font></a>      <font color="#112266"><b>for</b></font> (i1 = 0; i1 &lt; 4; i1++) <b>{</b>
<a name="323"><font color="#888888 "><i>  323   </i></font></a>        b_T02[i + (i0 &lt;&lt; 2)] += c_T02[i0 + (i1 &lt;&lt; 2)] * dv1[i1 + (i &lt;&lt; 2)];
<a name="324"><font color="#888888 "><i>  324   </i></font></a>      <b>}</b>
<a name="325"><font color="#888888 "><i>  325   </i></font></a>    <b>}</b>
<a name="326"><font color="#888888 "><i>  326   </i></font></a>  <b>}</b>
<a name="327"><font color="#888888 "><i>  327   </i></font></a>
<a name="328"><font color="#888888 "><i>  328   </i></font></a>  dv3[0] = cos(R_x) * cos(R_y);
<a name="329"><font color="#888888 "><i>  329   </i></font></a>  dv3[3] = cos(R_x) * sin(R_y) * sin(R_z) - sin(R_x) * cos(R_z);
<a name="330"><font color="#888888 "><i>  330   </i></font></a>  dv3[6] = cos(R_x) * sin(R_y) * cos(R_z) + sin(R_x) * sin(R_z);
<a name="331"><font color="#888888 "><i>  331   </i></font></a>  dv3[1] = sin(R_x) * cos(R_y);
<a name="332"><font color="#888888 "><i>  332   </i></font></a>  dv3[4] = sin(R_x) * sin(R_y) * sin(R_z) + cos(R_x) * cos(R_z);
<a name="333"><font color="#888888 "><i>  333   </i></font></a>  dv3[7] = sin(R_x) * sin(R_y) * cos(R_z) - cos(R_x) * sin(R_z);
<a name="334"><font color="#888888 "><i>  334   </i></font></a>  dv3[2] = -sin(R_y);
<a name="335"><font color="#888888 "><i>  335   </i></font></a>  dv3[5] = cos(R_y) * sin(R_z);
<a name="336"><font color="#888888 "><i>  336   </i></font></a>  dv3[8] = cos(R_y) * cos(R_z);
<a name="337"><font color="#888888 "><i>  337   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="338"><font color="#888888 "><i>  338   </i></font></a>    <font color="#112266"><b>for</b></font> (i0 = 0; i0 &lt; 3; i0++) <b>{</b>
<a name="339"><font color="#888888 "><i>  339   </i></font></a>      R47[i + 3 * i0] = 0.0;
<a name="340"><font color="#888888 "><i>  340   </i></font></a>      <font color="#112266"><b>for</b></font> (i1 = 0; i1 &lt; 3; i1++) <b>{</b>
<a name="341"><font color="#888888 "><i>  341   </i></font></a>        R47[i + 3 * i0] += b_T02[i + (i1 &lt;&lt; 2)] * dv3[i1 + 3 * i0];
<a name="342"><font color="#888888 "><i>  342   </i></font></a>      <b>}</b>
<a name="343"><font color="#888888 "><i>  343   </i></font></a>    <b>}</b>
<a name="344"><font color="#888888 "><i>  344   </i></font></a>  <b>}</b>
<a name="345"><font color="#888888 "><i>  345   </i></font></a>
<a name="346"><font color="#888888 "><i>  346   </i></font></a>  theta6 = acos(-R47[7]);
<a name="347"><font color="#888888 "><i>  347   </i></font></a>
<a name="348"><font color="#888888 "><i>  348   </i></font></a>  <font color="#117755"><i>/*  以上求得theta6 */</i></font>
<a name="349"><font color="#888888 "><i>  349   </i></font></a>  <font color="#112266"><b>if</b></font> (fabs(theta6) &gt; 1.0E-6) <b>{</b>
<a name="350"><font color="#888888 "><i>  350   </i></font></a>    theta5 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(R47[8] / sin(theta6), R47[6] / sin(theta6));
<a name="351"><font color="#888888 "><i>  351   </i></font></a>    theta7 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(-R47[4] / sin(theta6), R47[1] / sin(theta6));
<a name="352"><font color="#888888 "><i>  352   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="353"><font color="#888888 "><i>  353   </i></font></a>    theta5 = 0.0;
<a name="354"><font color="#888888 "><i>  354   </i></font></a>
<a name="355"><font color="#888888 "><i>  355   </i></font></a>    <font color="#117755"><i>/*  真实机器人赋予上一时刻值 */</i></font>
<a name="356"><font color="#888888 "><i>  356   </i></font></a>    theta7 = <a href="#fcn_rt_atan2d_snf" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_rt_atan2d_snf');" target="_self"><font color="#1122aa">rt_atan2d_snf</font></a>(-R47[3], R47[5]);
<a name="357"><font color="#888888 "><i>  357   </i></font></a>  <b>}</b>
<a name="358"><font color="#888888 "><i>  358   </i></font></a>
<a name="359"><font color="#888888 "><i>  359   </i></font></a>  theta[0] = theta1;
<a name="360"><font color="#888888 "><i>  360   </i></font></a>  theta[1] = x - 0.16086957252543341;
<a name="361"><font color="#888888 "><i>  361   </i></font></a>  theta[2] = theta3;
<a name="362"><font color="#888888 "><i>  362   </i></font></a>  theta[3] = theta4;
<a name="363"><font color="#888888 "><i>  363   </i></font></a>  theta[4] = theta5;
<a name="364"><font color="#888888 "><i>  364   </i></font></a>  theta[5] = theta6;
<a name="365"><font color="#888888 "><i>  365   </i></font></a>  theta[6] = theta7;
<a name="366"><font color="#888888 "><i>  366   </i></font></a>
<a name="367"><font color="#888888 "><i>  367   </i></font></a>  <font color="#117755"><i>/*  [q1,q2,q3,q4,q5,q6,q7] = [theta1,theta2,theta3,theta4,theta5,theta6,theta7]; */</i></font>
<a name="368"><font color="#888888 "><i>  368   </i></font></a>  <font color="#117755"><i>/*  q1 = theta1; */</i></font>
<a name="369"><font color="#888888 "><i>  369   </i></font></a>  <font color="#117755"><i>/*  q2 = theta2; */</i></font>
<a name="370"><font color="#888888 "><i>  370   </i></font></a>  <font color="#117755"><i>/*  q3 = theta3; */</i></font>
<a name="371"><font color="#888888 "><i>  371   </i></font></a>  <font color="#117755"><i>/*  q4 = theta4; */</i></font>
<a name="372"><font color="#888888 "><i>  372   </i></font></a>  <font color="#117755"><i>/*  q5 = theta5; */</i></font>
<a name="373"><font color="#888888 "><i>  373   </i></font></a>  <font color="#117755"><i>/*  q6 = theta6; */</i></font>
<a name="374"><font color="#888888 "><i>  374   </i></font></a>  <font color="#117755"><i>/*  q7 = theta7; */</i></font>
<a name="375"><font color="#888888 "><i>  375   </i></font></a>  <font color="#117755"><i>/*  theta = theta*(180/pi); */</i></font>
<a name="376"><font color="#888888 "><i>  376   </i></font></a>  <font color="#117755"><i>/*  整个函数是end */</i></font>
<a name="377"><font color="#888888 "><i>  377   </i></font></a><b>}</b>
<a name="378"><font color="#888888 "><i>  378   </i></font></a>
<a name="379"><font color="#888888 "><i>  379   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="380"><font color="#888888 "><i>  380   </i></font></a><font color="#117755"><I> * File trailer for ik_7dof_fourier.c</i></font>
<a name="381"><font color="#888888 "><i>  381   </i></font></a><font color="#117755"><I> *</i></font>
<a name="382"><font color="#888888 "><i>  382   </i></font></a><font color="#117755"><I> * [EOF]</i></font>
<a name="383"><font color="#888888 "><i>  383   </i></font></a><font color="#117755"><I> */</i></font>
<a name="384"><font color="#888888 "><i>  384   </i></font></a></pre>
</td></tr></table>
</p>
</body>
</html>